/*
* Copyright 2008  Rice University
* All Rights Reserved
* 
*       Created by Erion Plaku, Lydia Kavraki
*       Physical and Biological Computing Group
*       Department of Computer Science
*       Rice University
*/

#include "OOPSMP/Utils/Geometry/Collision/PQPCollisionDetector/PQPCollisionDetectorArticulatedPart3D.H"

BeginImplementInstanceFactory(PQPCollisionDetectorArticulatedPart3D, CollisionDetectorFactory);
EndImplementInstanceFactory(PQPCollisionDetectorArticulatedPart3D);


/**
 *@brief Use the workspace to get the geometry of static and moving parts
 *@param workspace workspace
 */
void OOPSMP::PQPCollisionDetectorArticulatedPart3D::addWorkspace(OOPSMP::Workspace_t workspace)
{
#ifdef HAVE_PQP 
	Part_t        part          = NULL;
	const int     nrStaticParts = workspace->getNrStaticParts();
	const int     nrMovingParts = workspace->getNrMovingParts();
	PQPPart_t     pqpMovingPart = NULL;
	int           id            = 0;
	ArticulatedPart3D_t	ap3D	= NULL;

	m_pqpStaticPart          = (PQPPart_t) calloc(1, sizeof(PQPPart_struct_t));
	m_pqpStaticPart->m_model = new PQP_Model();
	m_pqpStaticPart->m_model->BeginModel();

	for(int i = 0; i < nrStaticParts; i++)
	{
		part = dynamic_cast<Part_t>(workspace->getStaticPart(i));
		id   = addTrianglesToPQPModel(m_pqpStaticPart, part, id);
	}

	m_pqpStaticPart->m_model->EndModel();
	m_pqpStaticPart->R[0][0] = 1;
	m_pqpStaticPart->R[0][1] = 0;
	m_pqpStaticPart->R[0][2] = 0;
	m_pqpStaticPart->R[1][0] = 0;
	m_pqpStaticPart->R[1][1] = 1;
	m_pqpStaticPart->R[1][2] = 0;
	m_pqpStaticPart->R[2][0] = 0;
	m_pqpStaticPart->R[2][1] = 0;
	m_pqpStaticPart->R[2][2] = 1;
	m_pqpStaticPart->T[0] = 0;
	m_pqpStaticPart->T[1] = 0;
	m_pqpStaticPart->T[2] = 0;

	for(int i = 0; i < nrMovingParts; i++)
	{	
		part = dynamic_cast<Part_t>(workspace->getMovingPart(i));
		ap3D = const_cast<ArticulatedPart3D_t>(dynamic_cast<ArticulatedPart3D_const_t>(part));

		if ( ap3D == NULL ) { 

			pqpMovingPart = (PQPPart_t) calloc(1, sizeof(PQPPart_struct_t));
			pqpMovingPart->m_model = new PQP_Model();
			pqpMovingPart->m_model->BeginModel();	
			addTrianglesToPQPModel(pqpMovingPart, part, 0);
			pqpMovingPart->m_model->EndModel();
			setPQPTransformation(pqpMovingPart, part->getTransformation());

			m_pqpMovingParts.push_back(pqpMovingPart);

			v_movingPartLength.push_back(1);

		}
		else {
			int n = ap3D->getLinkCount();
			Part_t p = NULL;
			int idm = 0;

			for (int k = 0; k < n; k++)
			{
				p = const_cast<Part_t>(ap3D->getLinkPart(k));

				pqpMovingPart = (PQPPart_t) calloc(1, sizeof(PQPPart_struct_t));
				pqpMovingPart->m_model = new PQP_Model();
				pqpMovingPart->m_model->BeginModel();	
				idm = addTrianglesToPQPModel(pqpMovingPart, p, idm);
				pqpMovingPart->m_model->EndModel();
				setPQPTransformation(pqpMovingPart, p->getTransformation());

				m_pqpMovingParts.push_back(pqpMovingPart);
			}

			v_movingPartLength.push_back(n);

			/*FILE *file = fopen( "PQP_Debug.txt", "a" );
			fprintf( file, "in addWorkspace: added AP with %d links; numTri == %d; m_pqpMovingParts == %d; v_movingPartLength == %d\n\n", n, idm, m_pqpMovingParts.size() , v_movingPartLength.size() );
			fclose(file);*/
		}
	}
#else
	printf("Error: missing PQP library ... aborting\n");
	assert(0);
#endif
}

/**
 *@brief Apply the transformation to the moving part
 *@param pid moving part id
 *@param trans transformation 
 */
void OOPSMP::PQPCollisionDetectorArticulatedPart3D::transformMovingPart(const int pid, const double * const trans)
{
	CollisionDetector::transformMovingPart(pid, trans);

	int offset = 0, currentPid = 0;

	while ( currentPid < pid )
		offset += v_movingPartLength.at(currentPid++);

	Part_t part = dynamic_cast<Part_t>(m_workspace->getMovingPart(pid));
	ArticulatedPart_t ap3D = const_cast<ArticulatedPart3D_t>(dynamic_cast<ArticulatedPart3D_const_t>(part));

	if ( ap3D == NULL )
		setPQPTransformation( m_pqpMovingParts[offset], trans);
	else {
		int length = v_movingPartLength.at(currentPid);
		for( int i = offset ; i < offset + length ; i++ ) {
			setPQPTransformation( m_pqpMovingParts[i], ap3D->getLinkPart(i-offset)->getTransformation());
		}
	}

}

/**
 *@brief Returns true iff the indicated moving parts are in collision with each other
 *@param pid1 moving part id
 *@param pid2 moving part id
 */
bool OOPSMP::PQPCollisionDetectorArticulatedPart3D::isMovingPartInCollisionWithSomeStaticPart(const int pid)
{
	m_numCollisionChecks++;

	int offset = 0, currentPid = 0;

	while ( currentPid < pid )
		offset += v_movingPartLength.at(currentPid++);

	Part_t p = dynamic_cast<Part_t>(m_workspace->getMovingPart(pid));
	ArticulatedPart_t ap3D = const_cast<ArticulatedPart3D_t>(dynamic_cast<ArticulatedPart3D_const_t>(p));

	if ( ap3D == NULL )
	{
		PQPPart_t part = m_pqpMovingParts[pid];
		PQP_CollideResult collide_result;

		PQP_Collide(&collide_result,
			part->R, part->T, part->m_model,
			m_pqpStaticPart->R, m_pqpStaticPart->T, m_pqpStaticPart->m_model, PQP_FIRST_CONTACT);    
		return (collide_result.Colliding() != 0);
	}
	else
	{
		int result = 0; // no collision
		PQP_CollideResult collide_result;
		int length = v_movingPartLength.at(currentPid);

		/*FILE *file = fopen( "PQP_Debug.txt", "a" );
		fprintf( file, "in check m-s: offset == %d, length == %d\n\n", offset, length );
		fclose(file);*/

		for( int i = offset ; i < offset + length && result == 0 ; i++ )
		{
			PQPPart_t part = m_pqpMovingParts[i];

			PQP_Collide(&collide_result,
				part->R, part->T, part->m_model,
				m_pqpStaticPart->R, m_pqpStaticPart->T, m_pqpStaticPart->m_model, PQP_FIRST_CONTACT);

			result = collide_result.Colliding();
		}

		return (result != 0);
	}
}

/**
 *@brief Returns true iff the indicated moving parts are in collision with each other
 *@param pid1 moving part id
 *@param pid2 moving part id
 */
bool OOPSMP::PQPCollisionDetectorArticulatedPart3D::isMovingPartInCollisionWithMovingPart(const int pid1, const int pid2)
{
	m_numCollisionChecks++;
#ifdef HAVE_PQP

	// fetch the part 1 and determine whether it's a regular Part3D or an ArticulatedPart3D
	int offset1 = 0, currentPid1 = 0;

	while ( currentPid1 < pid1 )
		offset1 += v_movingPartLength.at(currentPid1++);

	Part_t p1 = dynamic_cast<Part_t>(m_workspace->getMovingPart(pid1));
	ArticulatedPart_t ap3D_1 = const_cast<ArticulatedPart3D_t>(dynamic_cast<ArticulatedPart3D_const_t>(p1));

	// fetch the part 2 and determine whether it's a regular Part3D or an ArticulatedPart3D
	int offset2 = 0, currentPid2 = 0;

	while ( currentPid2 < pid2 )
		offset2 += v_movingPartLength.at(currentPid2++);

	Part_t p2 = dynamic_cast<Part_t>(m_workspace->getMovingPart(pid2));
	ArticulatedPart_t ap3D_2 = const_cast<ArticulatedPart3D_t>(dynamic_cast<ArticulatedPart3D_const_t>(p2));
	
	// check collision, according the types of the parts involved
	PQP_CollideResult collide_result;
	int result = 0;

	if ( ap3D_1 == NULL && ap3D_2 == NULL )
	{
		PQPPart_t part1 = m_pqpMovingParts[pid1];
		PQPPart_t part2 = m_pqpMovingParts[pid2];

		PQP_Collide(&collide_result,
			part1->R, part1->T, part1->m_model,
			part2->R, part2->T, part2->m_model, PQP_FIRST_CONTACT);  

		return collide_result.Colliding() != 0;
	}
	else if ( ap3D_1 == NULL && ap3D_2 != NULL )
	{
		PQPPart_t part1 = m_pqpMovingParts[pid1];

		int length2 = v_movingPartLength.at(currentPid2);

		for( int i = offset2 ; i < offset2 + length2 && result == 0 ; i++ )
		{
			PQPPart_t part2 = m_pqpMovingParts[i];

			PQP_Collide(&collide_result,
				part1->R, part1->T, part1->m_model,
				part2->R, part2->T, part2->m_model, PQP_FIRST_CONTACT);

			result = collide_result.Colliding();
		}

		return result != 0;
	}
	else if ( ap3D_1 != NULL && ap3D_2 == NULL )
	{
		PQPPart_t part2 = m_pqpMovingParts[pid2];

		int length1 = v_movingPartLength.at(currentPid1);

		for( int i = offset1 ; i < offset1 + length1 && result == 0 ; i++ )
		{
			PQPPart_t part1 = m_pqpMovingParts[i];

			PQP_Collide(&collide_result,
				part1->R, part1->T, part1->m_model,
				part2->R, part2->T, part2->m_model, PQP_FIRST_CONTACT);

			result = collide_result.Colliding();
		}

		return result != 0;
	}
	else if ( ap3D_1 != NULL && ap3D_2 != NULL )
	{
		int length1 = v_movingPartLength.at(currentPid1);
		int length2 = v_movingPartLength.at(currentPid2);

		for( int i = offset1 ; i < offset1 + length1 && result == 0 ; i++ )
		{
			PQPPart_t part1 = m_pqpMovingParts[i];

			for( int i = offset2 ; i < offset2 + length2 && result == 0 ; i++ )
			{
				PQPPart_t part2 = m_pqpMovingParts[i];

				PQP_Collide(&collide_result,
					part1->R, part1->T, part1->m_model,
					part2->R, part2->T, part2->m_model, PQP_FIRST_CONTACT);

				result = collide_result.Colliding();
			}
		}

		return result != 0;
	}

#else
    printf("Error: missing PQP library ... aborting\n");
    assert(0);
    return false;
#endif
}

/**
*@brief Add triangles of the workspace part to PQP part
*@param pqpPart PQP part
*@param part workspace part
*@param id id of workspace part
*/
int OOPSMP::PQPCollisionDetectorArticulatedPart3D::addTrianglesToPQPModel(PQPPart_t pqpPart, OOPSMP::Part_const_t part, const int id)
{
#ifdef HAVE_PQP

	int tri = 0;
	ArticulatedPart3D_t articulatedPart3D = const_cast<ArticulatedPart3D_t>(dynamic_cast<ArticulatedPart3D_const_t>(part));

	//FILE *file = fopen( "PQP_Debug.txt", "a" );
	
	if ( articulatedPart3D != NULL )
	{
		int n = articulatedPart3D->getLinkCount();

		//fprintf( file, "in addTrianglesToPQPModel: ARTICULATED\n" );

		for (int k = 0; k < n; k++)
		{
			Part3D_const_t part3D = dynamic_cast<Part3D_const_t>(articulatedPart3D->getLinkPart(k));
			const int      ntri   = part3D->getNrTriangles();
			double         vtri[9];

			for(int i = 0; i < ntri; i++)
			{
				part3D->getTriangleVertices(i, vtri);
				pqpPart->m_model->AddTri(vtri, &(vtri[3]), &(vtri[6]), id + tri + i);
			}
			tri += ntri;
		}
	}
	else
	{
		Part3D_const_t part3D = dynamic_cast<Part3D_const_t>(part);
		const int      ntri   = part3D->getNrTriangles();
		double         vtri[9];

		//fprintf( file, "in addTrianglesToPQPModel: REGULAR\n" );

		for(int i = 0; i < ntri; i++)
		{
			part3D->getTriangleVertices(i, vtri);
			pqpPart->m_model->AddTri(vtri, &(vtri[3]), &(vtri[6]), id + i);
		}

		tri = ntri;
	}

	// fprintf( file, "in addTrianglesToPQPModel: triangles added == %d\n", tri );
	// fclose(file);

	return id + tri;
#else
	printf("Error: missing PQP librarry ... aborting\n");
	assert(0);
	return 0;
#endif
}

/**
*@brief Set the transformation of the PQP part as specified by <em>trans</em>
*@param pqpPart PQP part
*@param trans transformation
*/
void OOPSMP::PQPCollisionDetectorArticulatedPart3D::setPQPTransformation(PQPPart_t pqpPart, const double * const trans)
{
	pqpPart->R[0][0] = trans[ 0];
	pqpPart->R[0][1] = trans[ 1];
	pqpPart->R[0][2] = trans[ 2];
	pqpPart->R[1][0] = trans[ 4];
	pqpPart->R[1][1] = trans[ 5];
	pqpPart->R[1][2] = trans[ 6];
	pqpPart->R[2][0] = trans[ 8];
	pqpPart->R[2][1] = trans[ 9];
	pqpPart->R[2][2] = trans[10];	
	pqpPart->T[0]    = trans[ 3];
	pqpPart->T[1]    = trans[ 7];
	pqpPart->T[2]    = trans[11];
}










